#include "slepbase.h"
#include "epph.h"

#include <assert.h>
#include <math.h>


int LeastR(slep_matrix* A, slep_vector* y, double z, OPTS *opts, 
			slep_vector* x, slep_vector* funVal, slep_vector* valueL){

	slep_vector *ATy, *b1, *Ax, *x1;
	double rsL2, lambda, lambda_max;
	int i;

	assert( y->dim == A->m);
	assert( z >= 0.0);

	sll_opts(opts);

	initNormalization(A,opts);

	//assert( !(A->sparsity==SlepDense && opts->nFlag!=0) );

	Ax = slepAllocDenseVector(A->m);
	ATy = slepAllocDenseVector(A->n);
	b1 = slepAllocDenseVector(y->dim);

	NORMALIZEDMTV(A,y,ATy,opts,b1);

	if( hasGot(opts,SlepOPT_rsL2) )
		(rsL2=opts->rsL2), assert( rsL2 >= 0.0);
	else
		rsL2 = 0.0;

	if( opts->rFlag ==  0)
		lambda = z;
	else{
		assert( z >= 0.0 && z <= 1.0);
		lambda_max = slep_damax(ATy);
		lambda = z * lambda_max;
		rsL2 = rsL2 * lambda_max;
	}

	if( opts->init ==  2)
		slep_zerov(x);
	else{
		if(hasGot(opts,SlepOPT_x0)){
			assert(opts->x0->dim == A->n);
			slep_dvcopy(opts->x0,x);
		}else
			slep_dvcopy(ATy, x);
	}

	Ax = slepAllocDenseVector(A->m);
	x1 = slepAllocDenseVector(x->dim);

	NORMALIZEDMV(A,x,Ax,opts,x1,i);

	if( opts->init== 0){
		double x_norm, x_2norm;
		x_norm = slep_dasum(x);
		x_2norm = slep_ddot(x,x);
		if( x_norm >= 1e-6){
			;
		}
	}

	/* The main program */
	/* The Armijo Goldstein line search scheme + accelerated gradient descent */

	if( opts->mFlag == 0 && opts->lFlag == 0){
		int bFlag, iterStep;
		double L, l_sum, r_sum;
		slep_vector *xp, *xxp, *Axp, *s, *As, *v, *ATAs, *g, *Av, *Axy;
		double alpha, alphap, beta;

		bFlag=0;
		L = 1.0 + rsL2;

		xp = slepCopyToNewVector(x);
		Axp = slepCopyToNewVector(Ax);

		s = slepAllocDenseVector(x->dim);
		v = slepAllocDenseVector(x->dim);
		g = slepAllocDenseVector(x->dim);
		ATAs = slepAllocDenseVector(x->dim);
		As = slepAllocDenseVector(A->m);
		Av = slepAllocDenseVector(A->m);
		Axy = slepAllocDenseVector(A->m);

		xxp = slepAllocDenseVector(x->dim);
		slep_zerov(xxp);

		alphap=0.;
		alpha=1.;

		for( iterStep=0; iterStep < opts->maxIter; iterStep++){
	
			beta = (alphap-1.0)/alpha;

			slep_dvcopy(x,s);
			slep_daxpy(beta,xxp,s);

			slep_dvcopy(Ax,As);
			slep_daxpby(-beta, Axp, 1.0+beta, As);

			NORMALIZEDMTV(A,As, ATAs, opts, b1);

			slep_dvcopy(ATAs,g);
			slep_daxpy(-1.0,ATy,g);
			slep_daxpy(rsL2,s,g);

			slep_dvcopy(x,xp);
			slep_dvcopy(Ax,Axp);

			for(;;){
				slep_dvcopy(s,v);
				slep_daxpy(-1.0/L,g,v);

				//x = sign(v).* max(abs(v) - lambda/L, 0); 
				for(i=0; i< x->dim; i++)
					x->data.vctr[i] = (v->data.vctr[i] / fabs(v->data.vctr[i]))
									* max( (fabs(v->data.vctr[i]) - lambda/L) , 0);


				slep_dvcopy(x,v);
				slep_daxpy(-1.0,s,v);

				NORMALIZEDMV(A,x,Ax,opts,x1,i);

				slep_dvcopy(Ax,Av);
				slep_daxpy(-1.0,As,Av);

				r_sum = slep_ddot(v,v);
				l_sum = slep_ddot(Av,Av);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if( l_sum <= r_sum * (L - rsL2))
					break;
				else
					L = max( 2.0*L, rsL2 + l_sum / r_sum );
			}

			valueL->data.vctr[iterStep] = L;
			alphap = alpha;
			alpha = (1.0+sqrt(4.0*alpha*alpha + 1.0))/2.0;

			slep_dvcopy(x,xxp);
			slep_daxpy(-1.0,xp,xxp);

			slep_dvcopy(Ax,Axy);
			slep_daxpy(-1.0,y,Axy);

			funVal->data.vctr[iterStep] = 0.5*(slep_ddot(Axy,Axy)+ rsL2 * slep_ddot(x,x) + slep_dasum(x) * 2.0 * lambda );

			if(bFlag)
				break;

			if( opts->tFlag == 0){
				if( iterStep >= 1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dnrm2(xxp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dnrm2(xxp) <= (opts->tol) * max(slep_dnrm2(xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeVector(xp);
		slepFreeVector(xxp);
		slepFreeVector(Ax);
		slepFreeVector(Axp);
		slepFreeVector(s);
		slepFreeVector(As);
		slepFreeVector(v);
		slepFreeVector(ATAs);
		slepFreeVector(g);
		slepFreeVector(Axy);
		funVal->dim = iterStep;
		valueL->dim = iterStep;
	}


	/* Reformulated problem + Nemirovski's scheme */

	if( opts->mFlag == 1 && opts->lFlag == 0){
		int bFlag, iterStep;
		double L, l_sum, r_sum;
		slep_vector *xp, *xxp, *Axp, *s, *As, *v, *ATAs, *g, *Av, *Axy;
		slep_vector *t, *tp, *s_t, *u, *v_t;
		double alpha, alphap, beta;

		bFlag=0;
		L = 1.0 + rsL2;

		xp = slepCopyToNewVector(x);
		Axp = slepCopyToNewVector(Ax);

		s = slepAllocDenseVector(x->dim);
		v = slepAllocDenseVector(x->dim);
		g = slepAllocDenseVector(x->dim);
		ATAs = slepAllocDenseVector(x->dim);
		As = slepAllocDenseVector(A->m);
		Av = slepAllocDenseVector(A->m);
		Axy = slepAllocDenseVector(A->m);
		t = slepAllocDenseVector(x->dim);
		tp = slepAllocDenseVector(x->dim);
		s_t = slepAllocDenseVector(x->dim);
		u = slepAllocDenseVector(x->dim);
		v_t = slepAllocDenseVector(x->dim);

		xxp = slepAllocDenseVector(x->dim);
		slep_zerov(xxp);

		for(i=0; i < x->dim; ++i)
			t->data.vctr[i] = fabs(x->data.vctr[i]);
		tp = slepCopyToNewVector(t);

		alphap=0.;
		alpha=1.;

		for( iterStep=0; iterStep < opts->maxIter; iterStep++){
	
			beta = (alphap-1.0)/alpha;

			slep_dvcopy(x,s);
			slep_daxpy(beta,xxp,s);

			slep_dvcopy(t, s_t);
			slep_daxpby(-beta,tp,1.0+beta,s_t);

			slep_dvcopy(Ax,As);
			slep_daxpby(-beta, Axp, 1.0+beta, As);

			NORMALIZEDMTV(A,As, ATAs, opts, b1);

			slep_dvcopy(ATAs,g);
			slep_daxpy(-1.0,ATy,g);
			slep_daxpy(rsL2,s,g);

			slep_dvcopy(x,xp);
			slep_dvcopy(Ax,Axp);
			slep_dvcopy(t,tp);

			for(;;){
				slep_dvcopy(s,u);
				slep_daxpy(-1.0/L,g,u);

				DMEMSET(v->data.vctr,-lambda/L,v->dim,i);
				slep_daxpy(1.0,s_t,v);

				ep1R(x->data.vctr,t->data.vctr,u->data.vctr,v->data.vctr,x->dim);
				
				slep_dvcopy(x,v);
				slep_daxpy(-1.0,s,v);

				slep_dvcopy(t,v_t);
				slep_daxpy(-1.0,s_t,v_t);

				NORMALIZEDMV(A,x,Ax,opts,x1,i);

				slep_dvcopy(Ax,Av);
				slep_daxpy(-1.0,As,Av);

				r_sum = slep_ddot(v,v) + slep_ddot(v_t,v_t);
				l_sum = slep_ddot(Av,Av) + rsL2*slep_ddot(v,v);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if( l_sum <= r_sum*L )
					break;
				else
					L = max( 2.0*L, l_sum / r_sum );
			}

			valueL->data.vctr[iterStep] = L;
			alphap = alpha;
			alpha = (1.0+sqrt(4.0*alpha*alpha + 1.0))/2.0;

			slep_dvcopy(x,xxp);
			slep_daxpy(-1.0,xp,xxp);

			slep_dvcopy(Ax,Axy);
			slep_daxpy(-1.0,y,Axy);

			funVal->data.vctr[iterStep] = 0.5*(slep_ddot(Axy,Axy)+ rsL2 * slep_ddot(x,x) + slep_dsum(t) * 2.0 * lambda );

			if(bFlag)
				break;

			if( opts->tFlag == 0){
				if( iterStep >= 1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if( sqrt(slep_ddot(xxp,xxp)+slep_ddot(t,t)+slep_ddot(tp,tp)-2.0*slep_ddot(t,tp)) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( 
					sqrt(slep_ddot(xxp,xxp)+slep_ddot(t,t)+slep_ddot(tp,tp)-2.0*slep_ddot(t,tp))
						<= 
					(opts->tol) *
							max( sqrt(slep_ddot(xp,xp)+slep_ddot(tp,tp)+slep_ddot(t,t)-2.0*slep_ddot(t,tp)),1)
					)
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeVector(xp);
		slepFreeVector(xxp);
		slepFreeVector(Ax);
		slepFreeVector(Axp);
		slepFreeVector(s);
		slepFreeVector(As);
		slepFreeVector(v);
		slepFreeVector(ATAs);
		slepFreeVector(g);
		slepFreeVector(Axy);
		slepFreeVector(t);
		slepFreeVector(tp);
		slepFreeVector(s_t);
		slepFreeVector(u);
		slepFreeVector(v_t);
		funVal->dim = iterStep;
		valueL->dim = iterStep;
	}


	/* adaptive line search */

	if( opts->mFlag == 1 && opts->lFlag == 1){
		int bFlag, iterStep;
		double L, l_sum, r_sum;
		slep_vector *xp, *xxp, *Axp, *s, *As, *v, *ATAs, *g, *Av, *Axy;
		slep_vector *t, *tp, *ATAx, *ATAxp;
		double alpha, alphap, beta, gamma, tao;
		slep_vector *s_t, *u, *v_t, *xnew, *tnew, *Axnew;

		bFlag=0;
		L = 1.0 + rsL2;

		gamma = 1.0;

		xp = slepCopyToNewVector(x);
		Axp = slepCopyToNewVector(Ax);

		s = slepAllocDenseVector(x->dim);
		v = slepAllocDenseVector(x->dim);
		g = slepAllocDenseVector(x->dim);
		ATAs = slepAllocDenseVector(x->dim);
		As = slepAllocDenseVector(A->m);
		Av = slepAllocDenseVector(A->m);
		Axy = slepAllocDenseVector(A->m);
		t = slepAllocDenseVector(x->dim);
		tp = slepAllocDenseVector(x->dim);
		ATAx = slepAllocDenseVector(x->dim);
		ATAxp = slepAllocDenseVector(x->dim);
		s_t = slepAllocDenseVector(x->dim);
		u = slepAllocDenseVector(x->dim);
		v_t = slepAllocDenseVector(x->dim);
		xnew = slepAllocDenseVector(x->dim);
		tnew = slepAllocDenseVector(x->dim);
		Axnew = slepAllocDenseVector(A->m);

		

		xxp = slepAllocDenseVector(x->dim);
		slep_zerov(xxp);

		for(i=0; i < x->dim; ++i)
			t->data.vctr[i] = fabs(x->data.vctr[i]);
		tp = slepCopyToNewVector(t);

		NORMALIZEDMTV(A,Ax,ATAx,opts,b1);

		for( iterStep=0; iterStep < opts->maxIter; iterStep++){
	
			slep_dvcopy(ATAx,ATAxp);

			NORMALIZEDMTV(A,Ax,ATAx,opts,b1);
			for(;;){
				if(iterStep !=0){
					alpha = (-gamma + sqrt(gamma*gamma + 4.0*L*gamma))/(2.0*L);
					beta = (gamma - gamma*alphap)/(alphap*gamma + alphap*L*alpha);

					slep_dvcopy(x,s);
					slep_daxpy(beta,xxp,s);

					slep_dvcopy(t,s_t);
					slep_daxpby(-beta,tp,1.0+beta,s_t);

					slep_dvcopy(Ax,As);
					slep_daxpby(-beta, Axp, 1.0+beta, As);

					slep_dvcopy(ATAx,ATAs);
					slep_daxpby(-beta,ATAxp, 1.0+beta, ATAx);
				}else{
					alpha = (-1.0 + sqrt(5.0))/2.0;
					beta = 0.0;
					slep_dvcopy(x,s);
					slep_dvcopy(t,s_t);
					slep_dvcopy(Ax,As);
					slep_dvcopy(ATAx,ATAs);
				}


				slep_dvcopy(ATAs,g);
				slep_daxpy(-1.0,ATy,g);
				slep_daxpy(rsL2,s,g);


				slep_dvcopy(s,u);
				slep_daxpy(-1.0/L,g,u);

				DMEMSET(v->data.vctr,-lambda/L,v->dim,i);
				slep_daxpy(1.0,s_t,v);

				ep1R(xnew->data.vctr,tnew->data.vctr,u->data.vctr,v->data.vctr,x->dim);
				
				slep_dvcopy(xnew,v);
				slep_daxpy(-1.0,s,v);

				slep_dvcopy(tnew,v_t);
				slep_daxpy(-1.0,s_t,v_t);

				NORMALIZEDMV(A,xnew,Axnew,opts,x1,i);

				slep_dvcopy(Axnew,Av);
				slep_daxpy(-1.0,As,Av);

				r_sum = slep_ddot(v,v) + slep_ddot(v_t,v_t);
				l_sum = slep_ddot(Av,Av) + rsL2*slep_ddot(v,v);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if( l_sum <= r_sum*L )
					break;
				else
					L = max( 2.0*L, l_sum / r_sum );
			}

			gamma = L * alpha * alpha;
			alphap = alpha;

			valueL->data.vctr[iterStep] = L;

			tao = L * r_sum / l_sum;
			if(tao >= 5.0)
				L*= 0.8;
			slep_dvcopy(x,xp);
			slep_dvcopy(xnew,x);

			slep_dvcopy(x,xxp);
			slep_daxpy(-1.0,xp,xxp);

			slep_dvcopy(Ax,Axp);
			slep_dvcopy(Axnew,Ax);

			slep_dvcopy(t,tp);
			slep_dvcopy(tnew,t);

			slep_dvcopy(Ax,Axy);
			slep_daxpy(-1.0,y, Axy);

			funVal->data.vctr[iterStep] = 0.5*(slep_ddot(Axy,Axy)+ rsL2 * slep_ddot(x,x) + slep_dsum(t) * 2.0 * lambda );

			if(bFlag)
				break;

			if( opts->tFlag == 0){
				if( iterStep >= 1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if( sqrt(slep_ddot(xxp,xxp)+slep_ddot(t,t)+slep_ddot(tp,tp)-2.0*slep_ddot(t,tp)) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( 
					sqrt(slep_ddot(xxp,xxp)+slep_ddot(t,t)+slep_ddot(tp,tp)-2.0*slep_ddot(t,tp))
						<= 
					(opts->tol) *
							max( sqrt(slep_ddot(xp,xp)+slep_ddot(tp,tp)+slep_ddot(t,t)-2.0*slep_ddot(t,tp)),1)
					)
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeVector(xp);
		slepFreeVector(xxp);
		slepFreeVector(Ax);
		slepFreeVector(Axp);
		slepFreeVector(s);
		slepFreeVector(As);
		slepFreeVector(v);
		slepFreeVector(ATAs);
		slepFreeVector(g);
		slepFreeVector(Axy);
		slepFreeVector(t);
		slepFreeVector(tp);
		slepFreeVector(s_t);
		slepFreeVector(u);
		slepFreeVector(v_t);

		slepFreeVector(xnew);
		slepFreeVector(tnew);
		slepFreeVector(Axnew);
		slepFreeVector(ATAx);
		slepFreeVector(ATAxp);

		funVal->dim = iterStep;
		valueL->dim = iterStep;
	}
	slepFreeVector(ATy);
	slepFreeVector(b1);
	slepFreeVector(Ax);
	slepFreeVector(x1);
	return 0;
}